package com.dizner.androidcartest;

import android.os.SystemClock;
import android.util.Log;

import java.io.IOException;
import java.util.concurrent.TimeUnit;

public class AutoPlayManager implements Hcsr04.OnHCSensorHasDataListener {
    private static final String TAG = "AutoPlayManager";
    private static AutoPlayManager autoPlayManager;
    private Hcsr04 hcsr04;

    private MotorHAT motorHAT;
    private RaspberryPI3CarDiver diver1;
    private RaspberryPI3CarDiver diver2;
    private static final long def_miles = 1000;//控制消息发送默认间隔时间
    private boolean enableForward = true;
    private boolean enableLeft = true;
    private boolean enableRight = true;
    private boolean enableBack = true;
    private boolean isForward;
    private boolean isRight;
    private boolean isLeft;
    private boolean isBack;

    private boolean autoRunning;
    private static final int back_default_time = 500;//后退

    private static final int left_right_default_time = 1000;//左右转

    private static final int around_default_time = 2000;//掉头

    private AutoPlayManager(final MotorHAT motorHAT) {
        this.motorHAT = motorHAT;
        initEngine();
        try {
            if (hcsr04 == null) {
                hcsr04 = Hcsr04.getInstance("", "BCM7", "BCM8");
//                hcsr04.register(this);
            }

        } catch (IOException e) {
            e.printStackTrace();
        }

        if (forwardThread == null) {
            forwardThread = new Thread() {
                @Override
                public void run() {
                    while (true) {
                        if (isAutoRunning()) {
                            if (hcsr04.getDistance() >= 25) {
                                motorHAT.getSteeringEngine().write(100);
                                forward(0);
                            } else {
                                // TODO: 2019/4/2 开始检查 左右位置的障碍物
                                AutoPlayManager.this.stop();

                                motorHAT.getSteeringEngine().write(180);

                                SystemClock.sleep(500);

                                float leftDistance = hcsr04.getDistance();
                                SystemClock.sleep(500);
                                motorHAT.getSteeringEngine().write(5);
                                SystemClock.sleep(500);
                                float rightDistance = hcsr04.getDistance();
                                SystemClock.sleep(500);
                                motorHAT.getSteeringEngine().write(100);

                                if (leftDistance >= 25) {
                                    back(back_default_time);
                                    left(left_right_default_time);
                                    motorHAT.getSteeringEngine().write(100);
                                    continue;
                                } else if (rightDistance >= 25) {
                                    back(back_default_time);
                                    right(left_right_default_time);
                                    motorHAT.getSteeringEngine().write(100);
                                    continue;
                                } else if (rightDistance < 25 && leftDistance < 25) {
                                    back(back_default_time);
                                    left(around_default_time);
                                    motorHAT.getSteeringEngine().write(100);
                                    continue;

                                } else {
                                    back(back_default_time);
                                    left(around_default_time);
                                    motorHAT.getSteeringEngine().write(100);
                                    continue;

                                }

//                                if (hcsr04.getDistance() > 25) {
//                                    back(500);
//                                    left(1000);
//                                    motorHAT.getSteeringEngine().write(100);
//                                    continue;
//                                }
//
//                                motorHAT.getSteeringEngine().write(5);
//
//                                if (hcsr04.getDistance() > 25) {
//                                    back(500);
//                                    right(1000);
//                                    motorHAT.getSteeringEngine().write(100);
//                                    continue;
//                                }

//                                back(1000);

                            }
                        } else {
                            diver1.stop();
                            diver2.stop();
                        }
                    }
                }
            };
        }
    }

    public static AutoPlayManager getInstance(MotorHAT motorHAT) {
        if (autoPlayManager == null) {
            autoPlayManager = new AutoPlayManager(motorHAT);
        }
        return autoPlayManager;
    }


    private void initEngine() {
        motorHAT.getSteeringEngine().write(100);
        diver1 = RaspberryPI3CarDiver.create(motorHAT, 1, 2);
        diver2 = RaspberryPI3CarDiver.create(motorHAT, 3, 4);
        diver1.stop();
        diver2.stop();
    }

    public void run() {
        autoRunning = true;
        if (forwardThread != null && !forwardThread.isAlive()) {
            forwardThread.start();
        }
    }

    @Override
    public void onHcSeneorHasData(String sensorName, float distance) {
        Log.i(TAG, "" + distance);
        enableForward = distance > 20;
    }

    private Thread forwardThread;

    private boolean backTempStatus;

    private void back(long jg) {
        if (!isBack) {
            isBack = true;
            diver1.back(def_miles);
            diver2.back(def_miles);
        }
        try {
            if (jg > 0) {
                TimeUnit.MILLISECONDS.sleep(jg);
                stop();
            }
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    private void forward(long jg) {

        if (!isForward) {
            isForward = true;
            diver1.forward(def_miles);
            diver2.forward(def_miles);
        }
        try {
            if (jg > 0) {
                TimeUnit.MILLISECONDS.sleep(jg);
                stop();
            }
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    private void left(long jg) {
        if (!isLeft) {
            isLeft = true;
            diver1.right(def_miles);
            diver2.right(def_miles);
        }
        try {
            if (jg > 0) {
                TimeUnit.MILLISECONDS.sleep(jg);
                stop();
            }
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    private void right(long jg) {
        if (!isRight) {
            isRight = true;
            diver1.left(def_miles);
            diver2.left(def_miles);
        }
        try {
            if (jg > 0) {
                TimeUnit.MILLISECONDS.sleep(jg);
                stop();
            }
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    public boolean isAutoRunning() {
        return autoRunning;
    }

    public void stop() {
        isForward = false;
        isRight = false;
        isLeft = false;
        isBack = false;
        diver1.stop();
        diver2.stop();
//        if (forwardThread != null) {
//            forwardThread.interrupt();
//        }
//        autoRunning = false;
    }
}
